#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "python_lidar.h"

#include "QImage"
#include <iostream>
#include <string.h>
#include "fr_armaturenbrett.h"
#include "handle.h"
#include "stdlib.h"
#include <string>
#include <math.h>
#include "position_call.h"
#include <QMouseEvent>
#include <QPainter>
#include <QTimerEvent>
#undef slots
#include <Python.h>
#define slots Q_SLOTS
using namespace std;
struct ctrl_flag flag = {0,0};

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
//    fr = new FrArmaturenbrett(this);
//    fr->move(250,500);
//    fr->resize(500,500);
//    fr->show();
    connect(&fps_timer,SIGNAL(timeout()),this,SLOT(VideoShow()));
    pr = new PositionDisplay(this);
    pr->move(1250,320);
    pr->resize(1000,1000);
    pr->show();

    target = new TargetDisplay(this);
    target->move(1250,320);
    target->resize(100,100);
    target->show();

    handle_left = new Handle(this);
    handle_left->move(10,700);
    handle_left->left_right_ctrl = 1;
    handle_left->show();

    handle_right = new Handle(this);
    handle_right->move(1030,700);
    handle_right->left_right_ctrl = 2;
    handle_right->show();
    // 处理摇杆信号
    QObject::connect(handle_left,SIGNAL(chassis_move_ctrl()),this,SLOT(Move_chassis()));
    QObject::connect(handle_right,SIGNAL(chassis_move_ctrl()),this,SLOT(Move_chassis_omega()));
    int max = 164;
    int min = 0;
    ui->slider_y->setMinimum(min);  // 最小值
    ui->slider_y->setMaximum(max);  // 最大值
    ui->slider_x->setMinimum(min);
    ui->slider_x->setMaximum(max);

    //查找可用的串口
    foreach (const QSerialPortInfo &info,QSerialPortInfo::availablePorts())
    {
        QSerialPort serial;
        serial.setPort(info);
        if(serial.open(QIODevice::ReadWrite))
        {
            ui->PortBox->addItem(serial.portName());   //通过combBox将串口号读出来
            serial.close();
        }
    }
    //设置波特率下拉菜单默认显示第0项
    ui->BaudBox->setCurrentIndex(0);

}

MainWindow::~MainWindow()
{
    delete ui;

}

void MainWindow::on_OpenSerialButton_clicked()
{

    if(ui->OpenSerialButton->text() == tr("Open"))
    {
        serial = new QSerialPort;
        //设置串口名
        serial->setPortName(ui->PortBox->currentText());
        //打开串口
        serial->open(QIODevice::ReadWrite);
        //设置波特率
        serial->setBaudRate(QSerialPort::Baud115200);//设置波特率为115200
        //设置数据位数
        ui->label_11->clear();
        switch (ui->BitBox->currentIndex())
        {
        case 8:
            serial->setDataBits(QSerialPort::Data8);//设置数据位8
            break;
        default:
            break;
        }
        //设置校验位
        switch (ui->ParityBox->currentIndex())
        {
        case 0:
            serial->setParity(QSerialPort::NoParity);
            break;
        default:
            break;
        }
        //设置停止位
        switch (ui->BitBox->currentIndex())
        {
        case 1:
            serial->setStopBits(QSerialPort::OneStop);//停止位设置为1
            break;
        case 2:
            serial->setStopBits(QSerialPort::TwoStop);
        default:
            break;
        }
        //设置流控制
        serial->setFlowControl(QSerialPort::NoFlowControl);//设置为无流控制

        //关闭设置菜单使能
        ui->PortBox->setEnabled(false);
        ui->BaudBox->setEnabled(false);
        ui->BitBox->setEnabled(false);
        ui->ParityBox->setEnabled(false);
        ui->StopBox->setEnabled(false);
        ui->OpenSerialButton->setText(tr("Close"));


        //连接信号槽
        QObject::connect(ui->SendButton,&QPushButton::clicked,this,&MainWindow::on_SendButton_clicked);
        QObject::connect(serial,&QSerialPort::readyRead,this,&MainWindow::ReadData);
        QObject::connect(ui->pB1,&QPushButton::clicked,this,&MainWindow::send1);//显示扫描后的积木
        QObject::connect(ui->pB2,&QPushButton::clicked,this,&MainWindow::python_lidar);
        QObject::connect(ui->pB3,&QPushButton::clicked,this,&MainWindow::send3);//将相对与机器人的坐标发送给单片机，根据全场定位换算出世界坐标系下的积木坐标，进行跑点
        QObject::connect(ui->pB4,&QPushButton::clicked,this,&MainWindow::send4);
        QObject::connect(ui->pB5,&QPushButton::clicked,this,&MainWindow::send5);
        QObject::connect(ui->pB6,&QPushButton::clicked,this,&MainWindow::send6);
        QObject::connect(ui->pB7,&QPushButton::clicked,this,&MainWindow::send7);
                       // QObject::connect(ui->pB8,&QPushButton::clicked,this,&MainWindow::send8);//显示地图
        QObject::connect(ui->pB9,&QPushButton::clicked,this,&MainWindow::send9);
        QObject::connect(ui->pB10,&QPushButton::clicked,this,&MainWindow::send10);
        QObject::connect(ui->pB11,&QPushButton::clicked,this,&MainWindow::send11);
        QObject::connect(ui->sy1,&QPushButton::clicked,this,&MainWindow::move_hand1);
        QObject::connect(ui->sy2,&QPushButton::clicked,this,&MainWindow::move_hand2);
        QObject::connect(ui->sy3,&QPushButton::clicked,this,&MainWindow::move_hand3);
        QObject::connect(ui->sy4,&QPushButton::clicked,this,&MainWindow::move_hand4);
        QObject::connect(ui->sy5,&QPushButton::clicked,this,&MainWindow::move_hand5);
        QObject::connect(ui->sx1,&QPushButton::clicked,this,&MainWindow::move_handx1);
        QObject::connect(ui->sx2,&QPushButton::clicked,this,&MainWindow::move_handx2);
        QObject::connect(ui->sx3,&QPushButton::clicked,this,&MainWindow::move_handx3);
        QObject::connect(ui->sx4,&QPushButton::clicked,this,&MainWindow::move_handx4);
        QObject::connect(ui->sx5,&QPushButton::clicked,this,&MainWindow::move_handx5);
        QObject::connect(ui->free1,&QPushButton::clicked,this,&MainWindow::move_free_open);
        QObject::connect(ui->free2,&QPushButton::clicked,this,&MainWindow::move_free_close);
        QObject::connect(ui->open_view,&QPushButton::clicked,this,&MainWindow::on_pushButton_open_clicked);
        QObject::connect(ui->close_view,&QPushButton::clicked,this,&MainWindow::on_pushButton_close_clicked);


    }
    else
    {
        //关闭串口
        serial->clear();
        serial->close();
        serial->deleteLater();

        //恢复设置使能
        ui->PortBox->setEnabled(true);
        ui->BaudBox->setEnabled(true);
        ui->BitBox->setEnabled(true);
        ui->ParityBox->setEnabled(true);
        ui->StopBox->setEnabled(true);
        ui->OpenSerialButton->setText(tr("Open"));
        QPixmap *pixmap3 = new QPixmap("C:\\Users\\86157\\Desktop\\kk.JPG");
        pixmap3->scaled(ui->label->size(), Qt::KeepAspectRatio);
        ui->label_11->setScaledContents(true);
        ui->label_11->setPixmap(*pixmap3);


    }
     QObject::connect(ui->pB8,&QPushButton::clicked,this,&MainWindow::send8);//显示地图





}
//读取接收到的信息
void MainWindow::ReadData()
{

    QByteArray buf;
    QTextEdit a;

    std::string str1;

    buf = serial->readAll();
    if(!buf.isEmpty())
    {
        QString str = ui->textEdit->toPlainText();
        str+=tr(buf);
        QString str_buf ;
        str_buf = tr(buf);
        str1 = str_buf.toStdString();
        //std::cout << "str:" << str1 << std::endl;
        if(flag.pos_send_flag == 1&&flag.send_finished == 0)
        {

             str_all = str_all+str1;
             std::string::iterator iter ;
             for(iter = str1.begin();iter != str1.end();iter++)
             {
                 if(*iter == '#')
                 {
                     flag.send_finished = 1;
                     std::cout << "str_all:" <<str_all <<std::endl;
                     position_get(str_all);
                     str_all.clear();
                 }
             }


        }
        ui->textEdit->clear();
        ui->textEdit->append(str);

    }
    buf.clear();
}

//发送按钮槽函数
void MainWindow::on_SendButton_clicked()
{
    serial->write(ui->textEdit_2->toPlainText().toLatin1());
}
void MainWindow::send1()                             ///显示霍夫圆检测后的地图
{
//    QPixmap *pixmap = new QPixmap("C:\\Users\\86157\\Downloads\\build-stm32-Desktop_Qt_5_14_2_MinGW_64_bit-Release\\release\\rp.png");
//    pixmap->scaled(ui->label->size(), Qt::KeepAspectRatio);
//    ui->picture_label->setScaledContents(true);
//    ui->picture_label->setPixmap(*pixmap);
}
void MainWindow::send2()                             ///发送指定的数据
{
    char a[]="b";
    serial->write(a);
}
void MainWindow::send3()                             //将相对与机器人的坐标发送给单片机，根据全场定位换算出世界坐标系下的积木坐标，进行跑点
{

    std::string pos_x , pos_y , pos,yaw;
    float yaw_get;
    yaw_get = atan2((real_y - robot_y) , (real_x - robot_x));
    yaw = std::to_string(yaw_get);
    pos_x = std::to_string(real_x);
    pos_y = std::to_string(real_y);
    pos="Go2Point " +pos_x+" "+pos_y+" "+yaw+" 0.1"+" 0";
    serial->write(pos.c_str());
}
void MainWindow::send4()                             ///发送指定的数据
{
    char a[]="d";
    serial->write(a);
}
void MainWindow::send5()                             ///发送指定的数据
{
    flag.pos_send_flag = 1;
    flag.send_finished = 0;
    char a[]="getposturexy";
    serial->write(a);
}
void MainWindow::send6()                             ///发送指定的数据
{
    char a[]="e";
    serial->write(a);
}
void MainWindow::send7()                             ///发送指定的数据
{
    ui->label_12->clear();
    this->Tim_id1 = startTimer(20);
    //this->Tim_id2 = startTimer(20);
    //this->Tim_id3 = startTimer(20);
}
void MainWindow::send8()                             ///显示扫描后的地图
{

//    QPixmap *pixmap = new QPixmap("C:\\Users\\86157\\Downloads\\build-stm32-Desktop_Qt_5_14_2_MinGW_64_bit-Release\\release\\rp.png");
//    pixmap->scaled(ui->label->size(), Qt::KeepAspectRatio);
//    ui->picture_label->setScaledContents(true);
//    ui->picture_label->setPixmap(*pixmap);


}
void MainWindow::send9()                             ///查看鼠标坐标
{
    std::cout << MainWindow::pic_x << " "<< MainWindow::pic_y <<std::endl;
    std::cout << "real world : " << real_x << " " << real_y <<std::endl;

}
void MainWindow::send10()                             ///发送指定的数据
{
//    char a[]="i";
//    serial->write(a);
    python_lidar();
    QPixmap *pixmap = new QPixmap("C:\\workspace\\bupt_PC\\rplidar.png");
    pixmap->scaled(ui->label->size(), Qt::KeepAspectRatio);
    ui->picture_label->setScaledContents(true);
    ui->picture_label->setPixmap(*pixmap);
}
void MainWindow::send11()                             ///发送指定的数据
{
//    QPixmap *pixmap = new QPixmap("C:\\Users\\86157\\Desktop\\co.JPG");
//    pixmap->scaled(ui->label->size(), Qt::KeepAspectRatio);
//    ui->label_11->setScaledContents(true);
//    ui->label_11->setPixmap(*pixmap);
}


void MainWindow::python_lidar()
{
    Py_Initialize();
    if(!Py_IsInitialized())
    {
       qDebug()<<"Python init fail!";
    }
    //设置python文件路径
   PyRun_SimpleString("import sys");
   PyRun_SimpleString("sys.path.append('./')");

   PyObject *pModule = PyImport_ImportModule("py_cpp");
       if(!pModule)
       {
           qDebug()<<"load pModule(py_cpp.py) fail!";

       }
    PyObject* argc = PyTuple_New(2);
    float now_x ,now_y;
    now_x = robot_x;
    now_y = robot_y;
    PyTuple_SetItem(argc,0,Py_BuildValue("i",now_x));
    PyTuple_SetItem(argc,1,Py_BuildValue("i",now_y));
    PyObject* pFun_circle= PyObject_GetAttrString(pModule,"circle_detect");
    if(!pFun_circle)
    {
       qDebug()<<"Get pFun_show(show) failed!";
    }
    PyEval_CallObject(pFun_circle,argc);
    QPixmap *pixmap = new QPixmap("C:\\Users\\yy\\rplidar.png");
    pixmap->scaled(ui->label->size(), Qt::KeepAspectRatio);
    ui->picture_label->setScaledContents(true);
    ui->picture_label->setPixmap(*pixmap);
}

void MainWindow::mousePressEvent(QMouseEvent *e)
{
    MainWindow::buffer_x = e->x();
    MainWindow::buffer_y = e->y();
    //pr->move(buffer_x,buffer_y);
    if(buffer_x < 1521 && buffer_x > 980 && buffer_y > 50 && buffer_y < 591)
    {
        target->move(buffer_x,buffer_y);
    }
    double x = MainWindow::buffer_x;
    double y = MainWindow::buffer_y;
    double tmp_x;
    double tmp_y;
    //获取到图片的坐标,将picture_label大小设置长宽比例与opencv中图片一致，然后通过比例转换确定位置
    //确定的opencv图片大小为1500*1500，picture_label大小为541*541
    double rate = (1500.0/ 541.0) ;
    pic_x = x - 980 ;
    pic_y = y - 50 ;
    tmp_x = pic_x * rate ;
    tmp_y = pic_y * rate ;
    //std::cout  << tmp_x <<" " <<rate<< std::endl;
    //这里时py_cpp.py中进行的换算，具体换算参照py_cpp.py
    real_x = (tmp_x-750)*10 ;
    real_y = (tmp_y-750)*10 ;



}
void MainWindow::position_get(std::string str)
{
    string target = "#";
    string temp_x;
    string temp_y;
    int trans_flag = 0;
    int pos = str.find(target);
    str = str.erase(pos,1);
    string::iterator iter;
    for(iter = str.begin();iter != str.end();iter++)
    {
        if(*iter == ',' )
        {
            trans_flag = 1;
        }
        else if(trans_flag == 0)
        {
            temp_x = temp_x + *iter;
        }
        else if(trans_flag == 1)
        {
            temp_y = temp_y + *iter;
        }
    }
    robot_x = stof(temp_x);
    robot_y = stof(temp_y);
    float trans_x;
    float trans_y;
    trans_x = (robot_x /10) + 750;
    trans_y = (robot_y /10) +750;
    double rate = (1500.0/ 541.0) ;
    trans_x = (trans_x / rate)+980;
    trans_y = (trans_y/rate)+50;

    pr->move(trans_x,trans_y);
    cout << "x:" << robot_x <<"y:" << robot_y <<endl;


}
void MainWindow::rpm_add()
{
    int rpm = fr->get_rpm();
    rpm += 100;
    MainWindow::fr->set_rpm(rpm);
}
void MainWindow::rpm_fr()
{
    int rpm = fr->get_rpm();
    rpm -= 100;
    MainWindow::fr->set_rpm(rpm);
}


void MainWindow::timerEvent(QTimerEvent *event)
{

    if(event->timerId()==Tim_id1)
    {
        python_lidar();
        cout<<"hello"<<endl;
        send5();
        //move_free_x();
    }

}
void MainWindow::move_hand1()
{
    int height = 0;
    ui->slider_y->setValue(height);
    char a[]="slider_y 1";
    serial->write(a);
}
void MainWindow::move_hand2()
{
    int height = 41;
    ui->slider_y->setValue(height);
    char a[]="slider_y 2";
    serial->write(a);
}
void MainWindow::move_hand3()
{
    int height = 82;
    ui->slider_y->setValue(height);
    char a[]="slider_y 3";
    serial->write(a);
}
void MainWindow::move_hand4()
{
    int height = 123;
    ui->slider_y->setValue(height);
    char a[]="slider_y 4";
    serial->write(a);
}
void MainWindow::move_hand5()
{
    int height = 164;
    ui->slider_y->setValue(height);
    char a[]="slider_y 5";
    serial->write(a);
}
void MainWindow::move_handx1()
{
    int height = 0;
    ui->slider_x->setValue(height);
    char a[]="slider_x 1";
    serial->write(a);
}
void MainWindow::move_handx2()
{
    int height = 41;
    ui->slider_x->setValue(height);
    char a[]="slider_x 2";
    serial->write(a);
}
void MainWindow::move_handx3()
{
    int height = 82;
    ui->slider_x->setValue(height);
    char a[]="slider_x 3";
    serial->write(a);
}
void MainWindow::move_handx4()
{
    int height = 123;
    ui->slider_x->setValue(height);
    char a[]="slider_x 4";
    serial->write(a);
}
void MainWindow::move_handx5()
{
    int height = 164;
    ui->slider_x->setValue(height);
    char a[]="slider_x 5";
    serial->write(a);

}

void MainWindow::move_free_x()
{

    if(free_flag)
    {
    int free_x ;
    free_x = ui->slider_x->value();

    std::string free_x_str = std::to_string(free_x);

    int free_y ;
    free_y = ui->slider_y->value();

    std::string free_y_str = std::to_string(free_y);
    std::string pos_free = "free_slider "+free_x_str+" "+free_y_str;
    serial->write(pos_free.c_str());

    }

}

void MainWindow::move_free_y()
{

    if(free_flag)
    {
    int free_x ;
    free_x = ui->slider_y->value();

    std::string free_x_str = std::to_string(free_x);
    std::string pos_free = "free_slider_y "+free_x_str;
    serial->write(pos_free.c_str());
    }

}

void MainWindow::move_free_open()
{
    free_flag = 1;
}
void MainWindow::move_free_close()
{
    free_flag = 0;
}

void MainWindow::on_pushButton_open_clicked()
{
    camera.open("http://10.128.246.2:8080/?action=stream");
    fps_timer.start(20);
}

void MainWindow::on_pushButton_close_clicked()
{
    fps_timer.stop();
    camera.release();

}
void MainWindow::VideoShow()
{

    //opencv获取的图像的数据格式是mat类型，在第一步骤中，需要转换为qt中可以显示的qimage类型，
    cv::Mat frame;
    camera.read(frame);
    //cv读取的图像为bgr格式，看起来奇怪所以改成rgb
    cvtColor(frame,frame,CV_BGR2RGB);
    QImage image((unsigned char *)(frame.data),frame.cols,frame.rows,QImage::Format_RGB888);
    ui->label_view->setPixmap(QPixmap::fromImage(image));
    ui->label_view->resize(image.width(),image.height());

}
void MainWindow::Move_chassis()
{
//    char a[] = "Chassis_move";
//    serial->write(a);
    //std::cout << "hello" <<endl;
    float ctrl_yaw;
    ctrl_yaw = handle_left->getKeyNum();
    std::cout << ctrl_yaw <<endl;
}

void MainWindow::Move_chassis_omega()
{
    float omega ;
    int direction ;
    direction = handle_right -> getKeyNum();
    if(direction) omega = 0.1;//可以修改，可以根据handleR来判断角速度的大小
    else omega = -0.1;
    std::cout << omega << endl;
}
